#include "util.h"

#include <tiros/tiros.h>

#define NUM_TASKS 5
  
#define STKSIZE             (TIROS_MIN_CTXT_SZ + 64)
/* Stacks for each of our tasks */
osword_t task_stack[NUM_TASKS][STKSIZE];



/* One CS, One MQ, One eflag, other task sleeps x amount of time and
 * forces interrupt, one idle task. */

static eflag_t  tst_eflag;
static csem_t tst_cs;



#define TST_QSZ 4
osword_t tst_Q[msgQ_MEMSZ(TST_QSZ)];


#define EVENT1 0x01


#define TEST1  0x01
#define TEST2  0x02


void platform_init(void);
void platform_call_ISR(void);

void wait(void)
{
	delayms(1000);
}


int isr_test_num;

void call_ISR(const int test_num)
{
	isr_test_num = test_num;
	platform_call_ISR();
	wait();
}


void isr_action(void)
{
	putstring("ISR: "); puthexchar(isr_test_num);
	putchar('\n');
	switch(isr_test_num) {

	case TEST1:
		csem_V(&tst_cs);
		(void) eflag_set(&tst_eflag, EVENT1, O_EFLAG_TRIGGER);
		break;


	case TEST2:
		(void) msgQ_send( (msgQ_t*)tst_Q, (osptr_t)(osptrword_t)3);
		break;

	default:
		break;
	}
}








void idle_task(void *dummy)
{
	while(1)
		wait();
}


void eflag_task(void *dummy)
{
	int16_t rc;
	putstring("Eflag task starting\n");
	while(1) {
		rc = eflag_wait(&tst_eflag, EVENT1, 0, O_EFLAG_OR);
		if (rc == SUCCESS) {
			putstring("eflag rcvd\n");
		} else {
			putstring("eflag task: wait failed\n");
		}
	}
}

void csem_task(void *dummy)
{
	csemval_t rc;
	putstring("csem task starting\n");
	while(1) {
		rc = csem_P(&tst_cs, 0, 0);
		if (rc == 0) {
			putstring("csem_P success\n");
		} else {
			putstring("csem_P failed\n");
		}
	
	}
}



void msgq_task(void *dummy)
{
	int8_t rc;
	osptr_t tmp;
	putstring("msgq task starting\n");
	while(1) {
		rc = msgQ_recv( (msgQ_t*)tst_Q, 0, 0, &tmp);
		if (rc == SUCCESS) 
			putstring("msg rcvd\n");
		else 
			putstring("msg rcv failed\n");
	}
}


void notify_task(void *dummy)
{
	trtime_t timeoutval;
	secs_to_trtime(2, &timeoutval);

	while(1) {
		(void) os_wake_at(&timeoutval, O_RELATIVE_TIME);
		putstring("Forcing ISR test1\n");
		call_ISR( TEST1);

		(void) os_wake_at(&timeoutval, O_RELATIVE_TIME);
		putstring("Forcing ISR test2\n");
		call_ISR(TEST2);

	}
}







int main(void) 
{
	tid_t ef_tid, mq_tid, cs_tid, notify_tid, idle_tid;

	platform_init();
	/* -------------------------------------------------- */
	os_init();


	/* Priority is as follows:
	 * 0: eflag_task
	 * 1: msgq_task
	 * 2: csem_task
	 * 3: notify_task
	 * 4: idle_task
	 */


	ef_tid = os_task_create(eflag_task , (osptr_t)0,
				task_stack[0], STKSIZE, 0);


	mq_tid = os_task_create(msgq_task , (osptr_t)0,
				task_stack[1], STKSIZE, 1);


	cs_tid = os_task_create(csem_task , (osptr_t)0,
				task_stack[2], STKSIZE, 2);

	notify_tid = os_task_create(notify_task , (osptr_t)0,
				task_stack[3], STKSIZE, 3);
	idle_tid = os_task_create(idle_task , (osptr_t)0,
				task_stack[4], STKSIZE, 4);

	if (ef_tid == ILLEGAL_ELEM || mq_tid == ILLEGAL_ELEM ||
	    cs_tid == ILLEGAL_ELEM || notify_tid == ILLEGAL_ELEM ||	    
	    idle_tid == ILLEGAL_ELEM) {
		putstring("Task create failed\n");
	}


	eflag_init( &tst_eflag, 0);
	csem_init( &tst_cs, 0, 2);


	msgQ_init( (msgQ_t*) tst_Q, TST_QSZ);


	os_start();

	putstring("Started OS: Shouldn't reach here\n");
	while (1);
}







